/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright, Nick Johnson, Josep Bosch
*
*********************************************************************/

// Define DEBUG_OUTPUT to enable PRINTLN_DEBUG output when not using ROS.
// ROS debug level output is toggled at runtime using rxconsole.
//#define DEBUG_OUTPUT

// ================================================================== includes

#include <math.h>

#include "pilot_node.h"

using namespace std;

using namespace matchmaker;

// =================================================================== defines

// =================================================================== methods

// ---------------------------------------------------------------------------
bool Autopilot::init(int argc, char * argv[])
{
	PRINTLN_DEBUG("init entered");

	if (!getINIInt(m_updatePeriod, "updatePeriod", "AUTOPILOT")) return false;
	if (!getINIInt(m_navIdleTimeout, "navIdleTimeout", "AUTOPILOT")) return false;
	if (!getINIInt(m_diagPeriod,  "diagPeriod", "AUTOPILOT")) return false;

	if (!getINIFloat(m_transitOnXVel, "transitOnXVel", "AUTOPILOT")) {
		m_transitOnXVel = 0;
	}
	m_transitOffXVel = m_transitOnXVel * 2/3;

	if (!getINIFloat(m_transitOnXError, "transitOnXError", "AUTOPILOT")) {
		m_transitOnXError = 0;
	}
	m_transitOffXError = m_transitOnXError * 2/3;

	if (m_transitOnXVel > 0 && m_transitOnXError > 0) {
		PRINTLN_ERR("Cannot have both non-zero transitOnXVel AND m_transitOnXError; please choose one.");
		return false;
	}

	if (m_transitOnXVel > 0) {
		PRINTLN_INFO("transitOnXVel " << m_transitOnXVel << ", transitOffXVel " << m_transitOffXVel);
	}
	else if (m_transitOnXError > 0) {
		PRINTLN_INFO("transitOnXError " << m_transitOnXError << ", transitOffXError " << m_transitOffXError);
	}
	else {
		PRINTLN_INFO("Neither transitOnXVel nor transitOnXError set (> 0).");
		PRINTLN_INFO("Transit mode disabled; vehicle fixed in hover thruster arrangement.");
	}

	if (!m_pilot.init(m_rosHandle))
	{
		PRINTLN_ERR("Failed to initialise autopos.");
		return false;
	}
	
	m_thrusterMode = THRUSTER_MODE_HOVER;
	m_rosHandle->setParam("controlType","hover");

	m_updateCrono = new osl_core::Chrono();
	m_updateTimer.setTimer(1);
	
	m_manualMode = false;
	m_overrideForceMode = false;
	m_manualModeTimeout = 500;
	m_manualModeTimer.endTimer();
	
	m_velocityMode = false;
	m_velocityModeTimeout = 500;
	m_velocityModeTimer.endTimer();

	m_receivingNav = false;
	// Set nav idle timer to use wall time so we don't see spurious timeouts
	// when running very fast simulations (e.g. 32x).
	m_navIdleTimer.setUseWallTime(true);
	m_navIdleTimer.endTimer();

	m_autopilotActive = false;

	// Diag printing can be disabled with a zero diag period.
	if (m_diagPeriod != 0) {
		m_diagTimer.setTimer(1);
		PRINTLN_INFO("Periodic diag enabled with interval: " << m_diagPeriod);
	}
	else {
		PRINTLN_INFO("Periodic diag disabled due to zero diagPeriod.");
		m_diagTimer.endTimer();
	}
	
	// Subscribed topics, but provided services, according to SOA.
 	m_rosWorldReqSub = m_rosHandle->subscribe<auv_msgs::WorldWaypointReq>("pilot/world_waypoint_req",
 			10, &Autopilot::processROSWorldReq, this, ros::TransportHints().tcpNoDelay()); //communication between python is cpp is buggy with UDP comms 
 	m_rosBodyReqSub = m_rosHandle->subscribe<auv_msgs::BodyWaypointReq>("pilot/body_waypoint_req",
 			10, &Autopilot::processROSBodyReq, this, m_rosHints);
	m_rosVelocityReqSub = m_rosHandle->subscribe<auv_msgs::BodyVelocityReq>("pilot/body_velocity_req",
 			1, &Autopilot::processROSVelocityReq, this, m_rosHints);
 	m_rosStayReqSub = m_rosHandle->subscribe<auv_msgs::StayWaypointReq>("pilot/stay_waypoint_req",
 			10, &Autopilot::processROSStayReq, this, m_rosHints);
 	m_rosManualReqSub = m_rosHandle->subscribe<geometry_msgs::WrenchStamped>("pilot/manual_force_req",
 			1, &Autopilot::processROSManualReq, this, m_rosHints);
 	m_rosOverrideForceReqSub = m_rosHandle->subscribe<geometry_msgs::WrenchStamped>("pilot/override_force_req",
 			1, &Autopilot::processROSOverrideForceReq, this, m_rosHints);

 	// Published topics.
 	m_rosWaypointStsPub = m_rosHandle->advertise<auv_msgs::WaypointSts>("pilot/waypoint_sts", 10);
 	m_rosAxisForcePub = m_rosHandle->advertise<geometry_msgs::WrenchStamped>("pilot/force_req", 1);
 	m_rosThrustReqPub = m_rosHandle->advertise<auv_msgs::ThrusterForce>("pilot/thrust_req", 1);

 	if (m_useSoa)
 	{
 		// Register SOA services this node provides (both messages sources and sinks).
 		m_worldReqServiceAd = m_soaClient->advertiseService("WorldWaypointReq", m_rosWorldReqSub);

 		m_bodyReqServiceAd = m_soaClient->advertiseService("BodyWaypointReq", m_rosBodyReqSub);

 		m_velocityReqServiceAd = m_soaClient->advertiseService("BodyVelocityReq", m_rosVelocityReqSub);

 		m_stayReqServiceAd = m_soaClient->advertiseService("StayWaypointReq", m_rosStayReqSub);

 		m_manualReqServiceAd = m_soaClient->advertiseService("BodyForceReq", m_rosManualReqSub);

 		m_waypointStsServiceAd = m_soaClient->advertiseService("WaypointSts", m_rosWaypointStsPub);

 		// Update the status of the services for the first time.
 		updateServiceStatus();

 		// Try to find SOA dependencies for the first time.
 		findServices();
 		// And create a timer for periodic calls to findServices.
 		m_findServicesTimer = m_rosHandle->createTimer(ros::Duration(1.0),
 				boost::bind(&Autopilot::findServices, this));
 	}
 	else
 	{
 		// Try to connect to nav source manually, if not using SOA.
		m_rosNavStsSub = m_rosHandle->subscribe<auv_msgs::NavSts>("nav/nav_sts",
				1, &Autopilot::processROSNavSts, this, m_rosHints);
 	}

	PRINTLN_DEBUG("Initialisation complete.");

	return true;
}

// ---------------------------------------------------------------------------
void Autopilot::findServices()
{
	// Only query for a nav service if we aren't actively using and monitoring one already.
	// The status callback method below will reset the monitor if the service becomes unusable.

	if (!m_navServiceMon) {
		const std::string serviceName = "NavSts";
		ServiceDetailsPtr service = m_soaClient->findService(serviceName, true /* onlyUsableNow */);
		if (!service) {
			ROS_WARN_STREAM_THROTTLE(5, "Waiting for usable service: " << serviceName);
		}
		else {
			ROS_INFO_STREAM("Using new " << serviceName << " service: " << service->topic);
				m_rosNavStsSub = m_rosHandle->subscribe<auv_msgs::NavSts>(service->topic,
						1, &Autopilot::processROSNavSts, this, m_rosHints);

			// Create a monitor for the service, with callback now and on status change.
			m_navServiceMon = m_soaClient->monitorService(service,
					boost::bind(&Autopilot::navServiceStatusCallback, this, _1));
		}
	}
}

// ---------------------------------------------------------------------------
void Autopilot::navServiceStatusCallback(const matchmaker::ServiceDetails& service)
{
	if (!service.status.isUsable()) {
		ROS_WARN_STREAM("Lost navigation service " << service.id << ", state: "
				<< service.status.toString());
		// First shutdown the subscriber.
		m_rosNavStsSub.shutdown();
		// Then reset the pointer to the monitor object.
		m_navServiceMon.shutdown();
	}
}

// ---------------------------------------------------------------------------
void Autopilot::updateServiceStatus()
{
	ServiceStatus waypointSts = ServiceStatus::UNMET_DEPENDENCIES;
	ServiceStatus waypointCtrl = ServiceStatus::UNMET_DEPENDENCIES;
	ServiceStatus velocityCtrl = ServiceStatus::UNMET_DEPENDENCIES;
	ServiceStatus manualCtrl = ServiceStatus::AVAILABLE;

	if (!m_receivingNav) {
		// If we have no nav, manual control is all we can do.
		if (m_manualMode) {
			// If in manual mode, mark it as active, otherwise it defaults to availble.
			manualCtrl = ServiceStatus::ACTIVE;
		}
	}
	else {
		// Now check control modes, in this order: manual, velocity, waypoint

		if (m_manualMode) {
			// If manual mode is active, everything below is unavailable.
			manualCtrl = ServiceStatus::ACTIVE;
		}
		else if (m_velocityMode) {
			// If velocity mode is active, everything below is unavailable.
			velocityCtrl = ServiceStatus::ACTIVE;
		}
		else {
			// Velocity control is available during waypoint control.
			velocityCtrl = ServiceStatus::AVAILABLE;

			if (m_autopilotActive) {
				// FIXME: Waypoints don't timeout at the moment, so we can't properly
				// report SOA ACTIVE_BUSY state - have to report ACTIVE instead.
				// TODO: Separate statuses for world, body and stay request services?
				waypointCtrl = waypointSts = ServiceStatus::ACTIVE;

			}
			else {
				waypointCtrl = waypointSts = ServiceStatus::AVAILABLE;
			}
		}
	}

	m_manualReqServiceAd.setStatus(manualCtrl);
	m_velocityReqServiceAd.setStatus(velocityCtrl);

	m_worldReqServiceAd.setStatus(waypointCtrl);
	m_bodyReqServiceAd.setStatus(waypointCtrl);
	m_stayReqServiceAd.setStatus(waypointCtrl);

	m_waypointStsServiceAd.setStatus(waypointSts);
}

// ---------------------------------------------------------------------------
void Autopilot::processROSWorldReq(const auv_msgs::WorldWaypointReqConstPtr & msg)
{
	//PRINTLN_DEBUG("processROSWorldReq");

	// Waypoint commands are still processed in manual mode, but
	// the pilot won't act on them until manual mode is disabled.

	if (m_receivingNav && m_pilot.requestWorldWaypoint(msg->goal.requester, msg->goal.id, msg->goal.priority,
											msg->position.north, msg->position.east, msg->position.depth, msg->altitude, !msg->altitude_mode,
											osl_core::rad2deg(msg->orientation.pitch), osl_core::rad2deg(msg->orientation.yaw),
											msg->position_tolerance.x, msg->position_tolerance.y, msg->position_tolerance.z,
											osl_core::rad2deg(msg->orientation_tolerance.pitch), osl_core::rad2deg(msg->orientation_tolerance.yaw),
											!msg->disable_axis.x, !msg->disable_axis.y, !msg->disable_axis.z,
											!msg->disable_axis.pitch, !msg->disable_axis.yaw))
	{
		// Even if manual mode is active, we allow autopilot requests to be received.
		// They will come into effect once manual mode is disabled.
		m_autopilotActive = true;
		m_lastWaypointType = 1; // ALI_HOVER_WAYPOINT_TYPE_ABSOLUTE
	}

	// Send nav goal status regardless of whether request was accepted, as an ack/nack.
	sendNavGoalSts();
}

// ---------------------------------------------------------------------------
void Autopilot::processROSBodyReq(const auv_msgs::BodyWaypointReqConstPtr & msg)
{
	//PRINTLN_DEBUG("processROSBodyReq");

	// Waypoint commands are still processed in manual mode, but
	// the pilot won't act on them until manual mode is disabled.

	if (m_receivingNav && m_pilot.requestBodyWaypoint(msg->goal.requester, msg->goal.id, msg->goal.priority,
											msg->position.x, msg->position.y, msg->position.z, !msg->altitude_mode,
											osl_core::rad2deg(msg->orientation.pitch), osl_core::rad2deg(msg->orientation.yaw),
											msg->position_tolerance.x, msg->position_tolerance.y, msg->position_tolerance.z,
											osl_core::rad2deg(msg->orientation_tolerance.pitch), osl_core::rad2deg(msg->orientation_tolerance.yaw),
											!msg->disable_axis.x, !msg->disable_axis.y, !msg->disable_axis.z,
											!msg->disable_axis.pitch, !msg->disable_axis.yaw)) {

		m_autopilotActive = true;
		m_lastWaypointType = 2; // ALI_HOVER_WAYPOINT_TYPE_RELATIVE
	}

	// Send nav goal status regardless of whether request was accepted, as an ack/nack.
	sendNavGoalSts();
}

// ---------------------------------------------------------------------------
void Autopilot::processROSStayReq(const auv_msgs::StayWaypointReqConstPtr & msg)
{
	//PRINTLN_DEBUG("processROSStayReq");

	// Waypoint commands are still processed in manual mode, but
	// the pilot won't act on them until manual mode is disabled.

	if (m_receivingNav && m_pilot.requestStayWaypoint(msg->goal.requester, msg->goal.id, msg->goal.priority, !msg->altitude_mode,
								msg->position_tolerance.x, msg->position_tolerance.y, msg->position_tolerance.z,
								osl_core::rad2deg(msg->orientation_tolerance.pitch), osl_core::rad2deg(msg->orientation_tolerance.yaw),
								!msg->disable_axis.x, !msg->disable_axis.y, !msg->disable_axis.z,
								!msg->disable_axis.pitch, !msg->disable_axis.yaw)) {

		m_autopilotActive = true;
		m_lastWaypointType = 3; // ALI_HOVER_WAYPOINT_STAY
	}

	// Send nav goal status regardless of whether request was accepted, as an ack/nack.
	sendNavGoalSts();
}

// ---------------------------------------------------------------------------

void Autopilot::processROSNavSts(const auv_msgs::NavStsConstPtr & msg)
{
	//PRINTLN_DEBUG("Received ROS nav sts");

	m_pilot.setWorldPose(msg->position.north, msg->position.east,
							msg->position.depth, msg->altitude,
							osl_core::rad2deg(msg->orientation.roll),
							osl_core::rad2deg(msg->orientation.pitch),
							osl_core::rad2deg(msg->orientation.yaw));
	m_pilot.setBodyVelocity(msg->body_velocity.x, msg->body_velocity.y, msg->body_velocity.z,
							osl_core::rad2deg(msg->orientation_rate.roll),
							osl_core::rad2deg(msg->orientation_rate.pitch),
							osl_core::rad2deg(msg->orientation_rate.yaw));

	chooseThrusterModeFromNav(msg);

	// Set timeout so we stop if no nav for a while.
	m_receivingNav = true;
	m_navIdleTimer.setTimer(m_navIdleTimeout);
}

// ---------------------------------------------------------------------------
void Autopilot::processROSManualReq(const geometry_msgs::WrenchStampedConstPtr & msg)
{
	if (!m_manualMode) PRINTLN_INFO("Manual request message received - entering manual mode.");

	// ROS force messages are -1 to 1, whereas pilot is using legacy range -100 to 100, so scale values.
	m_pilot.manualForceRequest(msg->wrench.force.x*100, msg->wrench.force.y*100, msg->wrench.force.z*100,
			msg->wrench.torque.y*100, msg->wrench.torque.z*100);

	// Manual mode will override the autopilot while active.
	// When manual mode is disabled a new waypoint message (potentially the same request)
	// will need to be received before the pilot will enable autopilot mode.
	m_manualMode = true;
	m_autopilotActive = false;

	// Set timer to exit manual mode when requests stop.
	m_manualModeTimer.setTimer(m_manualModeTimeout);
}

void Autopilot::processROSOverrideForceReq(const geometry_msgs::WrenchStampedConstPtr & msg)
{
	if (m_manualMode) return;

	if (!m_overrideForceMode) {
		PRINTLN_INFO("First override force message received.");
	}

	// ROS force messages are -1 to 1, whereas pilot is using legacy range -100 to 100, so scale values.
	m_overrideForces.x = msg->wrench.force.x*100;
	m_overrideForces.y = msg->wrench.force.y*100;
	m_overrideForces.z = msg->wrench.force.z*100;
	m_overrideForces.pitch = msg->wrench.torque.y*100;
	m_overrideForces.yaw = msg->wrench.torque.z*100;

	// Manual mode will override the autopilot while active.
	// When manual mode is disabled a new waypoint message (potentially the same request)
	// will need to be received before the pilot will enable autopilot mode.
	m_overrideForceMode = true;

	// Set timer to exit manual mode when requests stop.
	m_manualModeTimer.setTimer(m_manualModeTimeout);

	if (!m_autopilotActive) m_updateTimer.setTimer(1);
}

void Autopilot::processROSVelocityReq(const auv_msgs::BodyVelocityReqConstPtr & msg)
{
	//if (m_manualMode) return;

	if (!m_velocityMode) PRINTLN_INFO("Velocity request message received - entering velocity mode.");

	if (m_receivingNav)
	{
		// Even if manual mode is active, we allow autopilot requests to be received.
		// They will come into effect once manual mode is disabled.We therefor do not change the status of m_autopilotActive
		// m_autopilotActive = true;
		m_lastWaypointType = 1; // ALI_HOVER_WAYPOINT_TYPE_ABSOLUTE
		//PRINTLN_INFO("Enable_axis.z " << !msg->disable_axis.z <<"\t msg->twist.linear.z: " << msg->twist.linear.z);
		//PRINTLN_INFO("Enable_axis.x " << !msg->disable_axis.x <<"\t msg->twist.linear.x: " << msg->twist.linear.x);
		m_pilot.velocityRequest(msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z, osl_core::rad2deg(msg->twist.angular.y), 
					osl_core::rad2deg(msg->twist.angular.z),!msg->disable_axis.x, !msg->disable_axis.y,
					!msg->disable_axis.z,!msg->disable_axis.pitch, !msg->disable_axis.yaw);
	}

	// Velocity mode will override the autopilot while active only for axis that are enabled.
	// When manual mode is disabled a new waypoint message (potentially the same request)
	// will need to be received before the pilot will enable autopilot mode.
	m_velocityMode = true;

	// Set timer to exit manual mode when requests stop.
	m_velocityModeTimer.setTimer(m_velocityModeTimeout);
}


// ---------------------------------------------------------------------------
void Autopilot::sendNavGoalSts()
{
	std::string requester;
	uint32_t goalID;
	uint32_t priority;
	m_pilot.getCurrentWaypoint(requester, goalID, priority);

	bool reportInPosition = false;
	if (m_autopilotActive) reportInPosition = m_pilot.inPosition();

	bool x, y, z, pitch, yaw;
	m_pilot.getAxisInPosition(x, y, z, pitch, yaw);

	auv_msgs::WaypointSts rosMsg;
	rosMsg.header.stamp = ros::Time::now();

	rosMsg.goal.requester = requester;
	rosMsg.goal.id = goalID;
	rosMsg.achieved = reportInPosition;

	m_rosWaypointStsPub.publish(rosMsg);
}

// ---------------------------------------------------------------------------
void Autopilot::sendForceMsg() {
	double x, y, z, pitch, yaw;
	m_pilot.getRequestedAxisForces(x, y, z, pitch, yaw);

	if (!m_manualMode && m_overrideForceMode) {
		// We only want to override an axis force when the override force is
		// not NAN. This is how the client signals to us which autopilot
		// forces are to be overridden.
		if (!isnan(m_overrideForces.x)) x = m_overrideForces.x;
		if (!isnan(m_overrideForces.y)) y = m_overrideForces.y;
		if (!isnan(m_overrideForces.z)) z = m_overrideForces.z;
		if (!isnan(m_overrideForces.pitch)) pitch = m_overrideForces.pitch;
		if (!isnan(m_overrideForces.yaw)) yaw = m_overrideForces.yaw;
	}

	geometry_msgs::WrenchStamped rosMsg;
	rosMsg.header.stamp = ros::Time::now();

	// ROS force messages are -1 to 1, whereas pilot is using legacy range -100 to 100, so scale values.
	rosMsg.wrench.force.x = x * 0.01;
	rosMsg.wrench.force.y = y * 0.01;
	rosMsg.wrench.force.z = z * 0.01;
	rosMsg.wrench.torque.x = 0;
	rosMsg.wrench.torque.y = pitch * 0.01;
	rosMsg.wrench.torque.z = yaw * 0.01;

	m_rosAxisForcePub.publish(rosMsg);
}

// ---------------------------------------------------------------------------
void Autopilot::sendZeroForceMsg()
{
	geometry_msgs::WrenchStamped rosMsg;
	rosMsg.header.stamp = ros::Time::now();

	rosMsg.wrench.force.x = 0;
	rosMsg.wrench.force.y = 0;
	rosMsg.wrench.force.z = 0;
	rosMsg.wrench.torque.x = 0;
	rosMsg.wrench.torque.y = 0;
	rosMsg.wrench.torque.z = 0;

	m_rosAxisForcePub.publish(rosMsg);
}

// ---------------------------------------------------------------------------
void Autopilot::chooseThrusterModeFromNav(const auv_msgs::NavStsConstPtr & msg)
{
	// This mode is only used in velocity mode
	if (m_velocityMode)
	{
		// Switching on velocity disabled
		if (m_transitOnXVel <= 0) return;
		if (m_thrusterMode == THRUSTER_MODE_TRANSIT) {
			// Possibly switch to hover mode if moving slowly enough along the body x axis
			if (abs(msg->body_velocity.x) < m_transitOffXVel) {
				ROS_INFO_STREAM("Autopilot: Switching to hover mode below x vel " << m_transitOffXVel << " (actual " <<  msg->body_velocity.x << ")");
				m_thrusterMode = THRUSTER_MODE_HOVER;
				m_rosHandle->setParam("controlType","hover");
			}
		}
		else {
			// Possibly switch to hover mode if moving quickly enough along the body x axis
			if (abs(msg->body_velocity.x) > m_transitOnXVel) {
				ROS_INFO_STREAM("Autopilot: Switching to transit mode above x vel " << m_transitOnXVel << " (actual " <<  msg->body_velocity.x << ")");
				m_thrusterMode = THRUSTER_MODE_TRANSIT;
				m_rosHandle->setParam("controlType","transit");
			}
		}	
		
	}
}
// ---------------------------------------------------------------------------
void Autopilot::chooseThrusterModeFromPoseError(const auv_pilot::BodyFrameNumeric<double>& poseError)
{
	// Switching on pose errordisabled or velocity_mode
	if (m_transitOnXError <= 0 || m_velocityMode) return;


	if (m_thrusterMode == THRUSTER_MODE_TRANSIT) {
		// Possibly switch to hover mode if close enough to the x set point in body frame
		if (abs(poseError.x) < m_transitOffXError) {
			ROS_INFO_STREAM("Autopilot: Switching to hover mode below x error " << m_transitOffXError << " (actual " << poseError.x << ")");
			m_thrusterMode = THRUSTER_MODE_HOVER;
			m_rosHandle->setParam("controlType","hover");
		}
	}
	else {
		// Possibly switch to transit mode if far enough from the x set point in body frame
		if (abs(poseError.x) > m_transitOnXError) {
			ROS_INFO_STREAM("Autopilot: Switching to transit mode above x error " << m_transitOnXError << " (actual " <<  poseError.x << ")");
			m_thrusterMode = THRUSTER_MODE_TRANSIT;
			m_rosHandle->setParam("controlType","transit");
		}
	}

}

// ---------------------------------------------------------------------------
void Autopilot::doWork()
{
	//Update PID values if they have changed.
	if (!m_pilot.updatePIDParams()) {
		PRINTLN_WARN("Failed to update PID gains!!!. Are they properly set?");
	}

	// Handle timeout since last navigation message.
	if (m_receivingNav && m_navIdleTimer.getTimer() == 0)
	{
		PRINTLN_WARN("*** No nav message for " << m_navIdleTimeout << " ms - stopping thrusters ***");
		m_receivingNav = false;
		// Don't actually clear the autopilotActive flag here, as we want it to continue with the
		// previous waypoint (if set), as soon as the nav returns.
		// If we want to time out the waypoint in some way because a new waypoint request hasn't
		// been received for a period, then that should be done elsewhere, regardless of nav state.
		sendZeroForceMsg();
		m_navIdleTimer.endTimer();
	}

	// Handle timeout since last manual request message.
	if (m_manualMode && m_manualModeTimer.getTimer() == 0)
	{
		PRINTLN_INFO("Manual request message timeout - leaving manual mode.");
		m_manualMode = false;
		sendZeroForceMsg();
		m_manualModeTimer.endTimer();

		if (m_autopilotActive) PRINTLN_INFO("Resuming autopilot control with previous waypoint.");
		else PRINTLN_INFO("Autopilot is currently inactive.");
	}

	// Handle timeout since last manual request message.
	if (m_overrideForceMode && m_manualModeTimer.getTimer() == 0)
	{
		PRINTLN_INFO("Override force request message timeout - cancelling override forces.");
		m_overrideForceMode = false;
		m_overrideForces.reset();
		m_manualModeTimer.endTimer();
	}
	
	// Handle timeout since last velocity request message
	
	if (m_velocityMode && m_velocityModeTimer.getTimer() == 0)
	{
		PRINTLN_INFO("Velocity request message timeout - leaving velocity mode.");
		m_velocityMode = false;
		m_autopilotActive = false; //Disable autopilot until new waypoint is received.
		sendZeroForceMsg();
		m_velocityModeTimer.endTimer();
		m_pilot.resetControlVelocity(); // resets enabled axis for velocity controller
		if (m_autopilotActive) PRINTLN_INFO("Resuming autopilot control with previous waypoint.");
		else PRINTLN_INFO("Autopilot is currently inactive.");
	}	
	
	// Periodic update
	if (m_updateTimer.getTimer() == 0)
	{
		m_updateTimer.setTimer(m_updatePeriod);
		
		const auv_pilot::BodyFrameNumeric<double> zeroError;
		// Always take priority on any other request
		if (m_manualMode)
		{
			// Cancel velocity request in manual mode, but leave waypoint valid for later.
			//m_velocityMode = false;

			//In manual mode the error we have is zero. This means controlType= hover
			chooseThrusterModeFromPoseError(zeroError);

			// In manual mode, we don't update the autopos, just send force messages.
			sendForceMsg();
		}
		// We now have a position request
		else if (m_autopilotActive && m_receivingNav)
		{
			// Note that m_overrideForceMode may also be true here, so override
			// forces may be in effect.

			// If the autopilot is active and we are receiving nav, update from the PIDs.
			m_pilot.update(m_updateCrono->ReadMs() / 1000.0);

			chooseThrusterModeFromPoseError(m_pilot.getPoseError());
			
			// if we have a velocity request, override the update for the axis that are enabled in velocity
			if (m_velocityMode)
			{
				// If velocity mode is active and we are receiving Nav, update speed and send forces as normal
				m_pilot.updateVelocity(m_updateCrono->ReadMs() / 1000.0);
			}	
			// If there are any force overrides, they will be applied within sendForceMsg().
			sendForceMsg();
			m_updateCrono->ReadMsAndReset();
		}
		// only velocity mode is enabled
		else if (m_velocityMode && m_receivingNav)
		{
			// If velocity mode is active and we are receiving Nav, update speed and send forces as normal
			m_pilot.updateVelocity(m_updateCrono->ReadMsAndReset() / 1000.0);

			// If there are any force overrides, they will be applied within sendAxisForceMsg().
			sendForceMsg();
		}
		
		else if (m_overrideForceMode) {
			// Autopilot is inactive, so set forces initially to zero.
			m_pilot.manualForceRequest(0, 0, 0, 0, 0);

			chooseThrusterModeFromPoseError(zeroError);

			// Now sendForceMsg() will apply the overrides.
			sendForceMsg();
		}
		else
		{
			chooseThrusterModeFromPoseError(zeroError);

			// Otherwise, our safest behaviour is to send zero force commands.
			sendZeroForceMsg();
		}
		
		// Period diagnostic messages after update, so the errors values are correct wrt latest nav.
	    if (m_diagTimer.getTimer() == 0)
		{
			m_diagTimer.setTimer(m_diagPeriod);

			m_pilot.printDiag();

			if (m_manualMode)
			{
				PRINTLN_INFO("Manual thruster mode.");
			}
			else if (m_velocityMode)
			{
				PRINTLN_INFO("Velocity mode.");
			}
			else  if (m_autopilotActive)
			{
				string s;

				s = "Autopilot active, ";
				if  (m_receivingNav)
				{
					if (m_pilot.inPosition())
					{
						s += "in position.";
					} else {
						s += "moving toward waypoint.";
					}
				}
				else
				{
					s += "but NO NAV - thrusters disabled.";
				}
				PRINTLN_INFO(s);
			}
			else if (!m_receivingNav)
			{
				PRINTLN_INFO("Autopilot inactive, and NO NAV.");
			}
			else
			{
				PRINTLN_INFO("Autopilot inactive.");
			}

			if (m_overrideForceMode) {
				PRINTLN_INFO("Override forces applied: "
						<<   "x " << m_overrideForces.x
						<< ", y " << m_overrideForces.y
						<< ", z " << m_overrideForces.z
						<< ", pitch " << m_overrideForces.pitch
						<< ", yaw " << m_overrideForces.yaw);
			}
		}
	}

	if (m_useSoa) updateServiceStatus();
}

// ---------------------------------------------------------------------------
void Autopilot::cleanup()
{
	PRINTLN_DEBUG("cleanup entered");

	ROS_INFO("Setting controlType parameter to hover mode");
	m_rosHandle->setParam("controlType","hover");
}

// ---------------------------------------------------------------------------
int main (int argc, char * argv[])
{
	Autopilot modCore;
	exit(modCore.main(argc, argv));
}
